# -*- coding: utf-8 -*-
#!/usr/bin/env python


import rospy
from mavros_msgs.msg import PositionTarget
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import String
import logging


# 设置日志配置
logging.basicConfig(
    filename='/home/tju/pos.log',  # 日志文件名
    level=logging.DEBUG,     # 设置日志级别
    format='%(asctime)s - %(levelname)s - %(message)s'  # 设置日志格式
)

# 设置无人机数量和阈值
uav_id = $id
land_height_threshold = 3.0  # 当高度大于这个值时执行着陆操作


# 定义处理Odometry消息的回调函数
def odometry_callback(message):
    # 获取 z 轴的高度
    x = message.pose.position.x
    y = message.pose.position.y
    z = message.pose.position.z

    # 如果高度大于设定阈值，执行着陆操作
    if z >= land_height_threshold  or x < -1.0 or y < -1.0 or z< -1.0:
        if z >= land_height_threshold  or x < -1.0 or y < -1.0:
            logging.info("uav_pos:{}".format((x,y,z)))
            cmd = 'AUTO.LAND'
            land_cmd.publish(cmd)
        elif z < -1.0 :
            command_msg = PositionTarget()
            command_msg.velocity.x = 0
            command_msg.velocity.y = 0
            command_msg.velocity.z = 0
            #速度控制
            command_msg.type_mask=0b111111000111
            stop_cmd.publish(command_msg)



if __name__ == "__main__":
    # 初始化 ROS 节点
    rospy.init_node('multi_drone_land_controller', anonymous=True)

    land_cmd=rospy.Publisher('/xtdrone/iris_'+str(uav_id)+'/cmd',String,queue_size=3)
    stop_cmd=rospy.Publisher('/iris_{}/mavros/setpoint_raw/local'.format(uav_id),PositionTarget,queue_size=3)

    # 创建每架无人机的Odometry话题订阅者
    rospy.Subscriber('/iris_{}/mavros/local_position/pose'.format(uav_id), PoseStamped, odometry_callback)

    # 保持节点运行
    rospy.spin()